{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "c9da8815",
   "metadata": {},
   "source": [
    "# Robotics, Vision & Control 3e: for Python\n",
    "## Chapter 14: Using Multiple Images"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "010c3265",
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "    import google.colab\n",
    "    print('Running on CoLab')\n",
    "    !pip install matplotlib\n",
    "    !pip install machinevision-toolbox-python\n",
    "    !pip install --no-deps rvc3python\n",
    "    COLAB = True\n",
    "except:\n",
    "    COLAB = False\n",
    "\n",
    "from IPython.core.interactiveshell import InteractiveShell\n",
    "InteractiveShell.ast_node_interactivity = \"last_expr_or_assign\"\n",
    "from IPython.core.display import HTML\n",
    "\n",
    "import RVC3 as rvc\n",
    "import sys, os.path\n",
    "sys.path.append(os.path.join(rvc.__path__[0], 'examples'))\n",
    "\n",
    "import numpy as np\n",
    "from scipy import linalg, stats\n",
    "import matplotlib.pyplot as plt\n",
    "import math\n",
    "from math import pi\n",
    "np.set_printoptions(\n",
    "    linewidth=120, formatter={\n",
    "        'float': lambda x: f\"{0:8.4g}\" if abs(x) < 1e-10 else f\"{x:8.4g}\"})\n",
    "np.random.seed(0)\n",
    "from machinevisiontoolbox.base import *\n",
    "from machinevisiontoolbox import *\n",
    "from spatialmath.base import *\n",
    "from spatialmath import *\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ca63d479",
   "metadata": {},
   "source": [
    "# 14.1 Point Feature Correspondence\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1e40d919",
   "metadata": {},
   "outputs": [],
   "source": [
    "view1 = Image.Read(\"eiffel-1.png\", mono=True)\n",
    "view2 = Image.Read(\"eiffel-2.png\", mono=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9aea85b0",
   "metadata": {},
   "outputs": [],
   "source": [
    "hf = view1.Harris(nfeat=150)\n",
    "view1.disp(block=None, darken=True); hf.plot();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4b561abe",
   "metadata": {},
   "outputs": [],
   "source": [
    "sf = view1.SIFT().sort().filter(minscale=10)[:150]\n",
    "view1.disp(block=None, darken=True); sf.plot(filled=True, color=\"y\", alpha=0.3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7784361e",
   "metadata": {},
   "outputs": [],
   "source": [
    "hf[0].descriptor.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a5e79d56",
   "metadata": {},
   "outputs": [],
   "source": [
    "hf[0].distance(hf[1], metric=\"ncc\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "81bb0604",
   "metadata": {},
   "outputs": [],
   "source": [
    "sf[0].descriptor.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8931d7b6",
   "metadata": {},
   "outputs": [],
   "source": [
    "sf[0].distance(sf[1], metric=\"L2\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7aaa4135",
   "metadata": {},
   "outputs": [],
   "source": [
    "sf1 = view1.SIFT()\n",
    "sf2 = view2.SIFT()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d572aac6",
   "metadata": {},
   "outputs": [],
   "source": [
    "matches = sf1.match(sf2);\n",
    "len(matches)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0e4ece8b",
   "metadata": {},
   "outputs": [],
   "source": [
    "matches[:5].list()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "df91ccaf",
   "metadata": {},
   "outputs": [],
   "source": [
    "matches.subset(100).plot(color=\"yellow\", block=None)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2287b6f1",
   "metadata": {},
   "outputs": [],
   "source": [
    "c = matches.correspondence();\n",
    "c[:, :5]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "38007889",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.hist(matches.distance, cumulative=True, density=True);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0317388b",
   "metadata": {},
   "outputs": [],
   "source": [
    "m = sf1.match(sf2, thresh=20);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ea9d898d",
   "metadata": {},
   "outputs": [],
   "source": [
    "m = sf1.match(sf2, sort=True)[:10];"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d9a5617c",
   "metadata": {},
   "outputs": [],
   "source": [
    "m = sf1.match(sf2, ratio=0.8)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5aecdcb2",
   "metadata": {},
   "outputs": [],
   "source": [
    "m = sf1.match(sf2, crosscheck=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7f24726f",
   "metadata": {},
   "source": [
    "# 14.2 Geometry of Multiple Views\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "68ae9920",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera1 = CentralCamera(name=\"camera 1\", f=0.002, imagesize=1000, \n",
    "                        rho=10e-6, pose=SE3.Tx(-0.1)*SE3.Ry(0.4))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6f917a76",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera2 = CentralCamera(name=\"camera 2\", f=0.002, imagesize=1000, \n",
    "                        rho=10e-6, pose=SE3.Tx(0.1)*SE3.Ry(-0.4))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9ec8e2dc",
   "metadata": {},
   "outputs": [],
   "source": [
    "ax = plotvol3([-0.4, 0.6, -0.5, 0.5, -0.2, 1]);\n",
    "camera1.plot(ax=ax, scale=0.15, shape=\"camera\", frame=True, color=\"blue\");\n",
    "camera2.plot(ax=ax, scale=0.15, shape=\"camera\", frame=True, color=\"red\");\n",
    "P=[0.5, 0.1, 0.8];\n",
    "plot_sphere(0.03, P, color=\"blue\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e6f059b0",
   "metadata": {},
   "outputs": [],
   "source": [
    "p1 = camera1.plot_point(P)\n",
    "p2 = camera2.plot_point(P)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ff1bc5a8",
   "metadata": {},
   "outputs": [],
   "source": [
    "e1 = camera1.plot_point(camera2.centre, \"kd\")\n",
    "e2 = camera2.plot_point(camera1.centre, \"kd\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "cbd83fec",
   "metadata": {},
   "source": [
    "## 14.2.1 The Fundamental Matrix\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2fc15a51",
   "metadata": {},
   "outputs": [],
   "source": [
    "F = camera1.F(camera2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a4d02dac",
   "metadata": {},
   "outputs": [],
   "source": [
    "e2h(p2).T @ F @ e2h(p1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "58a22631",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.linalg.matrix_rank(F)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "01f7995a",
   "metadata": {},
   "outputs": [],
   "source": [
    "e1h = linalg.null_space(F);\n",
    "e1h.T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c3319563",
   "metadata": {},
   "outputs": [],
   "source": [
    "e1 = h2e(e1h)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b98c887f",
   "metadata": {},
   "outputs": [],
   "source": [
    "e2h = linalg.null_space(F.T);\n",
    "e2 = h2e(e2h)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "08d79ab2",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.clf()\n",
    "with plt.ioff():\n",
    "    camera2.plot_epiline(F, p1, color=\"red\");\n",
    "    camera2.plot_point(camera1.centre, \"kd\");  # show the epipole\n",
    "    camera2.plot_point(P);                     # show the point"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a5157e4e",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.clf()\n",
    "with plt.ioff():\n",
    "    camera1.plot_epiline(F.T, p2, color=\"red\");\n",
    "    camera1.plot_point(camera2.centre, \"kd\");  # show the epipole\n",
    "    camera1.plot_point(P);                     # show the point\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3de91ce6",
   "metadata": {},
   "source": [
    "## 14.2.2 The Essential Matrix\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0388bd31",
   "metadata": {},
   "outputs": [],
   "source": [
    "E = camera1.E(F)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "777b39b7",
   "metadata": {},
   "outputs": [],
   "source": [
    "T_1_2 = camera1.decomposeE(E);\n",
    "T_1_2.printline(orient=\"camera\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5ddaed4f",
   "metadata": {},
   "outputs": [],
   "source": [
    "T_1_2_true = camera1.pose.inv() * camera2.pose;\n",
    "T_1_2_true.printline(orient=\"camera\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "81816dcb",
   "metadata": {},
   "outputs": [],
   "source": [
    "T_1_2_true.t / np.linalg.norm(T_1_2_true.t)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c48c21f9",
   "metadata": {},
   "outputs": [],
   "source": [
    "Q = [0, 0, 10];"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9a56e037",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera1.project_point(Q).T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3cc609b3",
   "metadata": {},
   "outputs": [],
   "source": [
    "for T in T_1_2:\n",
    " print(camera1.project_point(Q, pose=T).T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "38cc17f7",
   "metadata": {},
   "outputs": [],
   "source": [
    "T = camera1.decomposeE(E, Q);\n",
    "T.printline(orient=\"camera\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1c031dd4",
   "metadata": {},
   "source": [
    "## 14.2.3 Estimating the Fundamental Matrix from Real Image Data\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3e8e80f8",
   "metadata": {},
   "outputs": [],
   "source": [
    "P = np.random.uniform(low=-1, high=1, size=(3, 10)) + np.c_[0, 0, 3].T;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6cf95070",
   "metadata": {},
   "outputs": [],
   "source": [
    "p1 = camera1.project_point(P);\n",
    "p2 = camera2.project_point(P);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0a8e083b",
   "metadata": {},
   "outputs": [],
   "source": [
    "F, resid = CentralCamera.points2F(p1, p2)\n",
    "resid"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9a037198",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.linalg.matrix_rank(F)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3d5f7940",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.clf()\n",
    "with plt.ioff():\n",
    "    camera2.plot_point(P);\n",
    "    print(plt.isinteractive())\n",
    "    camera2.plot_epiline(F, p1, color=\"red\")\n",
    "    camera2.plot_point(camera1.centre, \"kd\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e967f667",
   "metadata": {},
   "outputs": [],
   "source": [
    "p2[:,[5, 6]] = p2[:,[6, 5]];"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b3e5238f",
   "metadata": {},
   "outputs": [],
   "source": [
    "_, resid = CentralCamera.points2F(p1, p2);\n",
    "resid"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "173b2e19",
   "metadata": {},
   "outputs": [],
   "source": [
    "CentralCamera.epidist(F, p1[:, 0], p2[:,0])\n",
    "CentralCamera.epidist(F, p1[:, 5], p2[:,5])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6d083250",
   "metadata": {},
   "outputs": [],
   "source": [
    "F, resid, inliers = CentralCamera.points2F(p1, p2, method=\"ransac\", \n",
    "                                           confidence=0.99, seed=0);\n",
    "resid"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "28c73355",
   "metadata": {},
   "outputs": [],
   "source": [
    "inliers"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "485ff681",
   "metadata": {},
   "outputs": [],
   "source": [
    "F, resid, inliers = CentralCamera.points2F(matches.p1, matches.p2, \n",
    "                                           method=\"ransac\", confidence=0.99);\n",
    "resid\n",
    "sum(inliers) / len(inliers)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3e3828e9",
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "sum(~inliers)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1615a9c7",
   "metadata": {},
   "outputs": [],
   "source": [
    "F, resid = matches.estimate(CentralCamera.points2F, method=\"ransac\", \n",
    "                            confidence=0.99, seed=0);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "772036ca",
   "metadata": {},
   "outputs": [],
   "source": [
    "matches\n",
    "matches[:10].list()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "63da3d8b",
   "metadata": {},
   "outputs": [],
   "source": [
    "matches.inliers.subset(100).plot(color=\"g\", block=None);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "747a6869",
   "metadata": {},
   "outputs": [],
   "source": [
    "matches.outliers.subset(100).plot(color=\"red\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1bd7b09e",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera = CentralCamera();\n",
    "with plt.ioff():\n",
    "    camera.disp(view1, block=None);\n",
    "    camera.plot_epiline(F.T, matches.inliers.subset(20).p2, color=\"black\");\n",
    "    epipole = h2e(linalg.null_space(F))\n",
    "    camera.plot_point(epipole, \"wd\");\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7cb31114",
   "metadata": {},
   "source": [
    "#### Excurse 14.3"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a7868265",
   "metadata": {},
   "outputs": [],
   "source": [
    "if COLAB:\n",
    "    !pip install --no-deps rvc3python\n",
    "\n",
    "from RVC3.examples import ransac_line\n",
    "\n",
    "x = np.arange(11);\n",
    "y = 3 * x - 10;\n",
    "nbad = 4;\n",
    "np.random.seed(1)  # set the random number generator seed\n",
    "bad = np.random.choice(len(x), nbad, replace=False)\n",
    "y[bad] = y[bad] + np.random.rand(nbad) * 10\n",
    "plt.plot(x, y, 'o')\n",
    "\n",
    "m, c, *_ = stats.linregress(x, y)\n",
    "plt.plot(x, m * x + c, 'r--');\n",
    "params, inliers = ransac_line.ransac_line(x, y)\n",
    "params\n",
    "inliers\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9074c960",
   "metadata": {},
   "source": [
    "## 14.2.4 Planar Homography\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0bb5c9af",
   "metadata": {},
   "outputs": [],
   "source": [
    "T_grid = SE3.Tz(1) * SE3.Rx(0.1) * SE3.Ry(0.2);\n",
    "P = mkgrid(3, 1.0, pose=T_grid);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bfaeba7a",
   "metadata": {},
   "outputs": [],
   "source": [
    "p1 = camera1.plot_point(P, \"o\");\n",
    "p2 = camera2.plot_point(P, \"o\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "da9dd39c",
   "metadata": {},
   "outputs": [],
   "source": [
    "H, resid = CentralCamera.points2H(p1, p2)\n",
    "H"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d1b4f565",
   "metadata": {},
   "outputs": [],
   "source": [
    "p2b = homtrans(H, p1);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ab2e0a84",
   "metadata": {},
   "outputs": [],
   "source": [
    "with plt.ioff():\n",
    "    camera2.plot_point(p2b, \"+\");\n",
    "    camera2.plot_point(P, \"o\");  # show original points"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "22df3313",
   "metadata": {},
   "outputs": [],
   "source": [
    "p1b = homtrans(np.linalg.inv(H), p1);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4a94cdc8",
   "metadata": {},
   "outputs": [],
   "source": [
    "Q = np.array([\n",
    "  [-0.2302,   -0.0545,    0.2537],\n",
    "  [ 0.3287,    0.4523,    0.6024],\n",
    "  [ 0.4000,    0.5000,    0.6000] ]);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "24eadf73",
   "metadata": {},
   "outputs": [],
   "source": [
    "plotvol3([-1, 1, -1, 1, 0, 2]);\n",
    "plot_sphere(0.05, P, color=\"blue\");\n",
    "plot_sphere(0.05, Q, color=\"red\");\n",
    "camera1.plot(color=\"blue\", frame=True);\n",
    "camera2.plot(color=\"red\", frame=True);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ae0b6ee1",
   "metadata": {},
   "outputs": [],
   "source": [
    "p1 = camera1.plot_point(np.hstack((P, Q)), \"o\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7c704abd",
   "metadata": {},
   "outputs": [],
   "source": [
    "p2 = camera2.plot_point(np.hstack((P, Q)), \"o\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e065af57",
   "metadata": {},
   "outputs": [],
   "source": [
    "p2h = homtrans(H, p1);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "deff11f5",
   "metadata": {},
   "outputs": [],
   "source": [
    "with plt.ioff():\n",
    "    camera2.plot_point(p2h, \"+\");\n",
    "    camera2.plot_point(p2);    # show projected points\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "910831c5",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.linalg.norm(homtrans(H, p1) - p2, axis=0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "31eb9368",
   "metadata": {},
   "outputs": [],
   "source": [
    "H, resid, inliers = CentralCamera.points2H(p1, p2, method=\"ransac\");\n",
    "resid\n",
    "inliers"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1f616597",
   "metadata": {},
   "outputs": [],
   "source": [
    "T, normals = camera1.decomposeH(H);\n",
    "T.printline(orient=\"camera\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "61252a96",
   "metadata": {},
   "outputs": [],
   "source": [
    "(camera1.pose.inv() * camera2.pose).printline(orient=\"camera\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "16a99668",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera1.pose.inv() * T_grid"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "44a2ceaf",
   "metadata": {},
   "outputs": [],
   "source": [
    "normals[1].T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b30b0fc6",
   "metadata": {},
   "outputs": [],
   "source": [
    "walls_l = Image.Read(\"walls-l.png\", reduce=2);\n",
    "walls_r = Image.Read(\"walls-r.png\", reduce=2);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0f529864",
   "metadata": {},
   "outputs": [],
   "source": [
    "sf_l = walls_l.SIFT();\n",
    "sf_r = walls_r.SIFT();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "70e49860",
   "metadata": {},
   "outputs": [],
   "source": [
    "matches = sf_l.match(sf_r);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "42794a1c",
   "metadata": {},
   "outputs": [],
   "source": [
    "H, resid = matches.estimate(CentralCamera.points2H, confidence=0.9, seed=0)\n",
    "matches"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "735b90a1",
   "metadata": {},
   "outputs": [],
   "source": [
    "walls_l.disp(block=None);\n",
    "plot_point(matches.inliers.p1, \"r.\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ddf08775",
   "metadata": {},
   "outputs": [],
   "source": [
    "not_plane = matches.outliers;"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c1d0a921",
   "metadata": {},
   "source": [
    "# 14.3 Sparse Stereo\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "603127ba",
   "metadata": {},
   "source": [
    "## 14.3.1 3D Triangulation\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "68204fef",
   "metadata": {},
   "outputs": [],
   "source": [
    "matches = sf_l.match(sf_r)\n",
    "F, resid = matches.estimate(CentralCamera.points2F, confidence=0.99, seed=0);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fe0c33b3",
   "metadata": {},
   "outputs": [],
   "source": [
    "matches = matches.inliers  # keep only the inliers"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7d38b648",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera = CentralCamera();\n",
    "camera.disp(walls_l, block=None);\n",
    "camera.plot_epiline(F.T, matches.subset(40).p2, \"yellow\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fb9e60bc",
   "metadata": {},
   "outputs": [],
   "source": [
    "f = walls_l.metadata(\"FocalLength\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d3ee056a",
   "metadata": {},
   "outputs": [],
   "source": [
    "name = walls_l.metadata(\"Model\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c8d2c0a1",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera = CentralCamera(name=name, imagesize=walls_l.shape, \n",
    "                       f=f/1000, rho=2*1.5e-6)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "880899dc",
   "metadata": {},
   "outputs": [],
   "source": [
    "E = camera.E(F)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "dba46111",
   "metadata": {},
   "outputs": [],
   "source": [
    "T_1_2 = camera.decomposeE(E, [0, 0, 10]);\n",
    "T_1_2.printline(orient=\"camera\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0b95702b",
   "metadata": {},
   "outputs": [],
   "source": [
    "t = T_1_2.t;\n",
    "s = 0.3 / t[0]  # estimate of translation scale factor\n",
    "T_1_2.t = s * t  # scaled translation\n",
    "T_1_2.printline(orient=\"camera\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4b9655ee",
   "metadata": {},
   "outputs": [],
   "source": [
    "ray1 = camera.ray(matches[0].p1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ef2b2918",
   "metadata": {},
   "outputs": [],
   "source": [
    "ray2 = camera.ray(matches[0].p2, pose=T_1_2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c59df6a6",
   "metadata": {},
   "outputs": [],
   "source": [
    "P, e = ray1.closest_to_line(ray2);\n",
    "P"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "35f71661",
   "metadata": {},
   "outputs": [],
   "source": [
    "e"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e5e68418",
   "metadata": {},
   "outputs": [],
   "source": [
    "ray1 = camera.ray(matches.p1);\n",
    "ray2 = camera.ray(matches.p2, pose=T_1_2);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9a53d71f",
   "metadata": {},
   "outputs": [],
   "source": [
    "len(ray1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4f6b909e",
   "metadata": {},
   "outputs": [],
   "source": [
    "P, e = ray1.closest_to_line(ray2);\n",
    "P.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5b63f9c4",
   "metadata": {},
   "outputs": [],
   "source": [
    "z = P[2, :];\n",
    "z.mean()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "014101d8",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.median(e)\n",
    "e.max()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f2f0acd5",
   "metadata": {},
   "outputs": [],
   "source": [
    "plotvol3();\n",
    "plt.plot(P[0,:], P[1,:], P[2,:], '.', markersize=2);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6071cf28",
   "metadata": {},
   "outputs": [],
   "source": [
    "if not COLAB:\n",
    "    walls_pcd = PointCloud(P)\n",
    "    walls_pcd.transform(SE3.Rx(pi));  # make y-axis upward\n",
    "    walls_pcd.disp(block=True)\n",
    "    walls_pcd = walls_pcd.remove_outlier(nb_points=10, radius=0.2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cdfeda1d",
   "metadata": {},
   "outputs": [],
   "source": [
    "if not COLAB:\n",
    "  colors = []\n",
    "  for m in matches:\n",
    "    colors.append(walls_l.image[int(m.p1[1]), int(m.p1[0]), :])\n",
    "  pcd = SE3.Rx(pi) * PointCloud(P, colors=np.array(colors).T)\n",
    "  pcd.disp(block=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e8e098f8",
   "metadata": {},
   "outputs": [],
   "source": [
    "p1_reproj = camera.project_point(P[:, 0]);\n",
    "p2_reproj = camera.project_point(P[:, 0], pose=T_1_2);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bbaa162f",
   "metadata": {},
   "outputs": [],
   "source": [
    "(p1_reproj - matches[0].p1).T\n",
    "(p2_reproj - matches[0].p2).T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6905fd4e",
   "metadata": {},
   "outputs": [],
   "source": [
    "bundle = BundleAdjust(camera);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "09c95849",
   "metadata": {},
   "outputs": [],
   "source": [
    "view0 = bundle.add_view(SE3(), fixed=True);\n",
    "view1 = bundle.add_view(SE3.Tx(0.3));"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "659eb77c",
   "metadata": {},
   "outputs": [],
   "source": [
    "for (Pj, mj) in zip(P[:, ::4].T, matches[::4]):\n",
    "  landmark = bundle.add_landmark(Pj)             # add vertex\n",
    "  bundle.add_projection(view0, landmark, mj.p1)  # add edge\n",
    "  bundle.add_projection(view1, landmark, mj.p2)  # add edge"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2e504125",
   "metadata": {},
   "outputs": [],
   "source": [
    "bundle"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "51a64abd",
   "metadata": {},
   "outputs": [],
   "source": [
    "bundle.plot()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "06de609b",
   "metadata": {},
   "outputs": [],
   "source": [
    "x = bundle.getstate();\n",
    "x.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b8ce0e60",
   "metadata": {},
   "outputs": [],
   "source": [
    "x[6:12]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "76b17fd9",
   "metadata": {},
   "outputs": [],
   "source": [
    "x[12:15]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7c31e3f5",
   "metadata": {},
   "outputs": [],
   "source": [
    "bundle.errors(x)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7a1ce02f",
   "metadata": {},
   "source": [
    "p, A, B = camera.derivatives(t, r, P);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "906652de",
   "metadata": {},
   "outputs": [],
   "source": [
    "x_new, resid = bundle.optimize(x);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d0c781e8",
   "metadata": {},
   "outputs": [],
   "source": [
    "bundle.setstate(x_new);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "24ee0683",
   "metadata": {},
   "outputs": [],
   "source": [
    "bundle.views[1].pose.printline(orient=\"camera\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7afb4316",
   "metadata": {},
   "outputs": [],
   "source": [
    "T_1_2.printline(orient=\"camera\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "513e7647",
   "metadata": {},
   "outputs": [],
   "source": [
    "bundle.landmarks[0].P"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6469a07f",
   "metadata": {},
   "outputs": [],
   "source": [
    "e = np.sqrt(bundle.getresidual());\n",
    "e.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "382d4cc9",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.median(e, axis=1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "14d491df",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.max(e, axis=1)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a9eb755a",
   "metadata": {},
   "source": [
    "# 14.4 Dense Stereo Matching\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "951cc46c",
   "metadata": {},
   "outputs": [],
   "source": [
    "rocks_l = Image.Read(\"rocks2-l.png\", reduce=2)\n",
    "rocks_r = Image.Read(\"rocks2-r.png\", reduce=2)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "91a314de",
   "metadata": {},
   "source": [
    "rocks_l.stdisp(rocks_r)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "813fdaae",
   "metadata": {},
   "outputs": [],
   "source": [
    "disparity, *_ = rocks_l.stereo_simple(rocks_r, hw=3, drange=[40, 90]);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "50d27fe4",
   "metadata": {},
   "outputs": [],
   "source": [
    "disparity.disp(colorbar=True);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4710613c",
   "metadata": {},
   "outputs": [],
   "source": [
    "disparity, similarity, DSI = rocks_l.stereo_simple(rocks_r, hw=3, drange=[40, 90])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3ec7b5c7",
   "metadata": {},
   "outputs": [],
   "source": [
    "DSI.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fddcc845",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.argmax(DSI, axis=2);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4e13fa1b",
   "metadata": {},
   "outputs": [],
   "source": [
    "similarity_values = np.max(DSI, axis=2);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f727ccb4",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.plot(DSI[439, 138, :], \"o-\");"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "de7198cb",
   "metadata": {},
   "source": [
    "## 14.4.1 Peak Refinement\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "64b9d10a",
   "metadata": {},
   "outputs": [],
   "source": [
    "disparity_refined, A = Image.DSI_refine(DSI)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8fd34620",
   "metadata": {},
   "source": [
    "## 14.4.2 Stereo Failure Modes\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "453c2d36",
   "metadata": {},
   "source": [
    "### 14.4.2.1 Multiple peaks\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2454d7de",
   "metadata": {},
   "source": [
    "### 14.4.2.2 Weak matching\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "77f99a77",
   "metadata": {},
   "outputs": [],
   "source": [
    "similarity.disp();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "14c66277",
   "metadata": {},
   "outputs": [],
   "source": [
    "similarity.choose(\"blue\", similarity < 0.6).disp();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bbd33664",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.hist(similarity.view1d(), 100, (0, 1), cumulative=True, density=True);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b88c67ea",
   "metadata": {},
   "source": [
    "### 14.4.2.3 Broad peak\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "12478101",
   "metadata": {},
   "source": [
    "### 14.4.2.4 Quantifying Failure Modes\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "267a19d4",
   "metadata": {},
   "outputs": [],
   "source": [
    "status = np.ones(disparity.shape);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3de0e800",
   "metadata": {},
   "outputs": [],
   "source": [
    "U, V = disparity.meshgrid()\n",
    "status[np.isnan(disparity.image)] = 5   # no similarity computed\n",
    "status[U <= 90] = 2                     # no overlap\n",
    "status[similarity.image < 0.6] = 3      # weak match\n",
    "status[A.image >= -0.1] = 4             # broad peak"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "715da02d",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.imshow(status);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2dad776a",
   "metadata": {},
   "outputs": [],
   "source": [
    "(status == 1).sum() / status.size * 100"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "99230531",
   "metadata": {},
   "outputs": [],
   "source": [
    "disparity_valid = disparity.choose(0, status!=1)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1ddccacd",
   "metadata": {},
   "source": [
    "### 14.4.2.5 Slicing the DSI\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2c35d335",
   "metadata": {},
   "outputs": [],
   "source": [
    "Image(DSI[100, :, :].T).disp();"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1877f7eb",
   "metadata": {},
   "source": [
    "### 14.4.2.6 Summary\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "53ef8a4b",
   "metadata": {},
   "source": [
    "### 14.4.2.7 Advanced Stereo Matching\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f705e997",
   "metadata": {},
   "outputs": [],
   "source": [
    "disparity_BM = rocks_l.stereo_BM(rocks_r, hw=3, drange=[40, 90], speckle=(200, 2))\n",
    "disparity_BM.disp();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0be9fd4a",
   "metadata": {},
   "outputs": [],
   "source": [
    "rocks_l.stereo_SGBM(rocks_r, hw=3, drange=[40, 90], speckle=(200, 2)).disp();"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9e3eb310",
   "metadata": {},
   "source": [
    "### 14.4.2.8 3D Reconstruction\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "eb0f847f",
   "metadata": {},
   "outputs": [],
   "source": [
    "di = disparity_BM.image * 2 + 274;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "386d1e10",
   "metadata": {},
   "outputs": [],
   "source": [
    "U, V = disparity_BM.meshgrid();\n",
    "u0, v0 = disparity.centre;\n",
    "f = 3740;   # pixels, according to Middlebury website\n",
    "b = 0.160;  # m, according to Middlebury website\n",
    "X = b * (U - u0) / di; Y = b * (V - v0) / di; Z = f * b / di;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9f908821",
   "metadata": {},
   "outputs": [],
   "source": [
    "fig, ax = plt.subplots(subplot_kw={\"projection\": \"3d\"})\n",
    "ax.plot_surface(X, Y, Z)\n",
    "ax.view_init(-100, -100)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8242e30d",
   "metadata": {},
   "outputs": [],
   "source": [
    "if not COLAB:\n",
    "    cam = CentralCamera(f=f, imagesize=rocks_l.shape);\n",
    "    pcd = PointCloud(Z, image=rocks_l, camera=cam, depth_trunc=1.9)\n",
    "    pcd *= SE3.Rx(pi);  # make y-axis upward\n",
    "    pcd.disp(block=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3f007b44",
   "metadata": {},
   "source": [
    "## 14.4.3 Image Rectification\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "27756c04",
   "metadata": {},
   "outputs": [],
   "source": [
    "walls_l = Image.Read('walls-l.png', reduce=2)\n",
    "walls_r = Image.Read('walls-r.png', reduce=2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "33145097",
   "metadata": {},
   "outputs": [],
   "source": [
    "sf_l = walls_l.SIFT()\n",
    "sf_r = walls_r.SIFT()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "16d3e7ee",
   "metadata": {},
   "outputs": [],
   "source": [
    "matches = sf_l.match(sf_r);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a28831d1",
   "metadata": {},
   "outputs": [],
   "source": [
    "F, resid = matches.estimate(CentralCamera.points2F, \n",
    "                            method=\"ransac\", confidence=0.95);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "94517533",
   "metadata": {},
   "outputs": [],
   "source": [
    "H_l, H_r = walls_l.rectify_homographies(matches, F)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cfcd00f0",
   "metadata": {},
   "outputs": [],
   "source": [
    "walls_l_rect = walls_l.warp_perspective(H_l)\n",
    "walls_r_rect = walls_r.warp_perspective(H_r)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3fa95b37",
   "metadata": {},
   "outputs": [],
   "source": [
    "if COLAB:\n",
    "    walls_l_rect.disp()\n",
    "    walls_r_rect.disp()\n",
    "else:\n",
    "    walls_l_rect.stdisp(walls_r_rect)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "959fe420",
   "metadata": {},
   "outputs": [],
   "source": [
    "walls_l_rect.stereo_SGBM(walls_r_rect, hw=7, drange=[180, 530], speckle=(50, 2)).disp();"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "56d4bd49",
   "metadata": {},
   "source": [
    "# 14.5 Anaglyphs\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "57bfd910",
   "metadata": {
    "lines_to_next_cell": 2
   },
   "outputs": [],
   "source": [
    "walls_l.anaglyph(walls_r, \"rc\").disp();"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e4240e55",
   "metadata": {},
   "source": [
    "# 14.7 Point Clouds\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9bd29ab2",
   "metadata": {},
   "outputs": [],
   "source": [
    "if not COLAB:\n",
    "    bunny_pcd = PointCloud.Read('data/bunny.ply')\n",
    "    bunny_pcd.disp(block=True)\n",
    "    pcd = bunny_pcd.voxel_grid(voxel_size=0.01).disp(block=True)\n",
    "    pcd = bunny_pcd.downsample_voxel(voxel_size=0.01)\n",
    "    pcd.normals(radius=0.1, max_nn=30)\n",
    "    pcd.disp(block=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4241fa19",
   "metadata": {},
   "source": [
    "## 14.7.1 Fitting a Plane\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f2d7d603",
   "metadata": {},
   "outputs": [],
   "source": [
    "if not COLAB:\n",
    "    pcd = walls_pcd\n",
    "    # plane, plane_pcd, pcd = pcd.segment_plane(distance_threshold=0.05, seed=0)\n",
    "    plane, plane_pcd, pcd = pcd.segment_plane(distance_threshold=0.05)\n",
    "    print(plane)\n",
    "\n",
    "    print(plane_pcd)\n",
    "\n",
    "    # plane, plane_pcd, pcd = pcd.segment_plane(distance_threshold=0.05, seed=0)\n",
    "    plane, plane_pcd, pcd = pcd.segment_plane(distance_threshold=0.05)\n",
    "    print(plane)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b5e9f2dd",
   "metadata": {},
   "source": [
    "## 14.7.2 Matching Two Sets of Points\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3f8fba16",
   "metadata": {},
   "outputs": [],
   "source": [
    "if not COLAB:\n",
    "    model = bunny_pcd.downsample_random(0.1, seed=0)\n",
    "\n",
    "    data = SE3.Trans(0.3, 0.4, 0.5) * SE3.Rz(50, unit=\"deg\") * bunny_pcd.downsample_random(0.05, seed=-1);\n",
    "\n",
    "    model.paint([0, 0, 1])  # blue\n",
    "    data.paint([1, 0, 0])   # red\n",
    "    (model + data).disp(block=True)\n",
    "\n",
    "    T, status = model.ICP(data, max_correspondence_distance=1, \n",
    "                    max_iteration=2000, relative_fitness=0, relative_rmse=0)\n",
    "    T.printline()\n",
    "\n",
    "    (model + T.inv() * data).disp(block=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3774dc75",
   "metadata": {},
   "source": [
    "# 14.8 Applications\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e04b99bc",
   "metadata": {},
   "source": [
    "## 14.8.1 Perspective Correction\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f636f819",
   "metadata": {},
   "outputs": [],
   "source": [
    "notredame = Image.Read(\"notre-dame.png\");\n",
    "notredame.disp();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ccfa3892",
   "metadata": {},
   "outputs": [],
   "source": [
    "# this won't work with Jupyter, use the pre-picked points defined in the next cell\n",
    "# picked_points = plt.ginput(4);\n",
    "# p1 = np.array(picked_points).T;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "01b94f68",
   "metadata": {},
   "outputs": [],
   "source": [
    "# coordinates of 4 points that mark the corners of a rectangle on the face of\n",
    "# the building\n",
    "p1 = np.array([\n",
    "        [ 44.1364,   94.0065,  537.8506,  611.8247], \n",
    "        [377.0654,  152.7850,  163.4019,  366.4486]]);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "93f8b9ea",
   "metadata": {},
   "outputs": [],
   "source": [
    "notredame.disp(block=None);\n",
    "plot_polygon(p1, filled=True, color=\"y\", alpha=0.4, linewidth=2);\n",
    "plot_point(p1, \"yo\");\n",
    "mn = p1.min(axis=1);\n",
    "mx = p1.max(axis=1);\n",
    "p2 = np.array([[mn[0], mn[0], mx[0], mx[0]], [mx[1], mn[1], mn[1], mx[1]]]);\n",
    "plot_polygon(p2, \"k--\", close=True, linewidth=2);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "07e2de3c",
   "metadata": {},
   "outputs": [],
   "source": [
    "H, _ = CentralCamera.points2H(p1, p2, method=\"leastsquares\")\n",
    "H"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "df2c322a",
   "metadata": {},
   "outputs": [],
   "source": [
    "notredame.warp_perspective(H).disp();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "53a3587b",
   "metadata": {},
   "outputs": [],
   "source": [
    "f = notredame.metadata(\"FocalLength\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "654c716c",
   "metadata": {},
   "outputs": [],
   "source": [
    "cam = CentralCamera(imagesize=notredame.shape, f=f/1000, sensorsize=[7.18e-3, 5.32e-3])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "69d81ecf",
   "metadata": {},
   "outputs": [],
   "source": [
    "pose, normals = cam.decomposeH(H)\n",
    "pose.printline(orient=\"camera\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d8d0c4a0",
   "metadata": {},
   "outputs": [],
   "source": [
    "normals[0].T"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a34a8584",
   "metadata": {},
   "source": [
    "## 14.8.2 Image Mosaicing\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ee49b24b",
   "metadata": {},
   "outputs": [],
   "source": [
    "images = ImageCollection(\"mosaic/aerial2-*.png\", mono=True);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "638f9427",
   "metadata": {},
   "outputs": [],
   "source": [
    "composite = Image.Zeros(2_000, 2_000)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8e500cc6",
   "metadata": {},
   "outputs": [],
   "source": [
    "composite.paste(images[0], (0, 0));\n",
    "composite.disp();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "03eb633b",
   "metadata": {},
   "outputs": [],
   "source": [
    "next_image = images[1]\n",
    "sf_c = composite.SIFT()\n",
    "sf_next= next_image.SIFT()\n",
    "match = sf_c.match(sf_next);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fd378806",
   "metadata": {},
   "outputs": [],
   "source": [
    "H, _ = match.estimate(CentralCamera.points2H, \"ransac\", confidence=0.99);\n",
    "H"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1556c50c",
   "metadata": {},
   "outputs": [],
   "source": [
    "tile, topleft, corners = next_image.warp_perspective(H, inverse=True, tile=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bffe9236",
   "metadata": {},
   "outputs": [],
   "source": [
    "composite.paste(tile, topleft, method=\"blend\");\n",
    "composite.disp();"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8aeafab8",
   "metadata": {},
   "source": [
    "## 14.8.3 Visual Odometry\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6b5c7795",
   "metadata": {},
   "outputs": [],
   "source": [
    "import mvtbdata.mvtb_load_image_data\n",
    "\n",
    "left = ZipArchive(\"bridge-l.zip\", filter=\"*.pgm\", mono=True, dtype=\"uint8\", \n",
    "                  maxintval=4095, roi=[20, 750, 20, 480]);\n",
    "len(left)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "209b56c8",
   "metadata": {},
   "outputs": [],
   "source": [
    "import mvtbdata.mvtb_load_image_data"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fb712713",
   "metadata": {},
   "outputs": [],
   "source": [
    "if not COLAB:  # simple animations do not work properly in Jupyter\n",
    "  for image in left:\n",
    "    #image.disp(reuse=True, block=None)\n",
    "    image.disp(reuse=True, fps=10, title=\"bridge-l\", matplotlib=False)\n",
    "  cv_destroy_window(\"bridge-l\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "ee74ea26",
   "metadata": {},
   "source": [
    "<div class=\"note\">\n",
    "\n",
    "The commented out code produces an animation for a Python script, however, using Jupyter it produces a set of separate images.  Use OpenCV instead (`matplotlib=False`) to display the animation in a separate window.\n",
    "</div>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "179b7457",
   "metadata": {},
   "outputs": [],
   "source": [
    "for image in left:\n",
    "   features = image.ORB(nfeatures=20)         # compute ORB features\n",
    "   image = image.colorize()                   # create RGB image\n",
    "   features.draw2(image, color='y');          # render ORB features into image\n",
    "   # image.disp(reuse=True, fps=20, matplotlib=False) # display the image\n",
    "   # features.plot()                          # overlay the features\n",
    "   # pause(0.05)\n",
    "\n",
    "   features.draw2(image, color='y');          # render ORB features into image\n",
    "   image.disp(reuse=True, fps=20, matplotlib=False) # display the image"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3e8ee133",
   "metadata": {},
   "outputs": [],
   "source": [
    "right = ZipArchive(\"bridge-r.zip\", mono=True, dtype=\"uint8\", \n",
    "                   maxintval=4095, roi=[20, 750, 20, 480]);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "487ee117",
   "metadata": {},
   "outputs": [],
   "source": [
    "%run -m visodom"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "08f61b2c",
   "metadata": {},
   "outputs": [],
   "source": [
    "ts = np.loadtxt(left.open(\"timestamps.dat\"));"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "dd5382f8",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.plot(np.diff(ts));"
   ]
  }
 ],
 "metadata": {
  "jupytext": {
   "cell_metadata_filter": "-all",
   "main_language": "python",
   "notebook_metadata_filter": "-all"
  },
  "kernelspec": {
   "display_name": "dev",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.9"
  },
  "vscode": {
   "interpreter": {
    "hash": "b7d6b0d76025b9176285a6442c3dd6dd39bcfe7241029b7898b7106bd5e9b472"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
